#!/usr/bin/perl

package tblas;

BEGIN 
{
  $pi = 3.1415926535897932384626433832795 ;
}

# Vectors

sub sum_vectors
{
# arguments: vector1 vector2
  my @result;
  my $n = @_ / 2;
  for (my $i = 0; $i < $n; ++$i) { $result[$i] = $_[$i] + $_[$n + $i]; }
  return @result;
}

sub dot_prod
{
# arguments: vector1, vector2
  my $result = 0;
  my $dim = @_ / 2;
  for (my $i = 0; $i < $dim; ++$i) { $result += $_[$i]*$_[$dim + $i]; }
  return $result;
}

sub vec_prod
{
# arguments: vector1, vector2
  return ($_[1]*$_[5] - $_[2]*$_[4], $_[2]*$_[3] - $_[0]*$_[5], $_[0]*$_[4] - $_[1]*$_[3]);
}

sub scale
{
# arguments: scale_factor, vector
  my @result;
  for (my $i = 0; $i < @_ - 1; ++$i) { $result[$i] = $_[ 1 + $i ] * $_[0]; }
  return @result;
}

sub vector_length
{
# arguments: vector
  return sqrt(dot_prod(@_, @_));
}

sub normalized_vector
{
# arguments: vector
  return scale(1.0 / vector_length( @_ ), @_);
}

sub get_perpendicular
{
# arguments: vector (3D)
  if ( dot_prod(1, 0, 0 , @_) != 0 )
  {
    return normalized_vector( vec_prod(1, 0, 0, @_) );
  } else {
    return normalized_vector( vec_prod(0, 1, 0, @_) );
  }
}

sub get_axis 
{
# returns direction
# arguments: vector_to vector_from
  return normalized_vector( sum_vectors(@_[0..2], scale(-1, @_[3..5])) );
}

sub distance
{
# returns distance between 2 vectors
# arguments: vec1 vec2
  my $n = @_ / 2;
  return vector_length(sum_vectors(@_[0..$n-1], scale(-1, @_[$n..$#_]) ) );  
}

sub compare_vectors
{
# arguments: vector1 vector2
# returns 1 if vectors are equal
  if ( distance(@_) < 0.0001 ) { return 1; } else { return 0; }
}

sub test_parallel
# returns 1 if vectors are parallel
# arguments: vec1 vec2
{
  my $n = @_ / 2;
  if (vector_length(vec_prod(@_[0..$n-1], @_[$n..$#_])) == 0) { return 1; } else {return 0;}
}

# Matrices

sub zero_matrix
{
# arguments: dimension1, dimension2
  my @result;
  for (my $i = 0; $i < $_[0]*$_[1]; ++$i) { $result[$i] = 0; }
  return @result;
}

sub identity_matrix
{
# arguments: dimension1
  my @result = zero_matrix($_[0], $_[0]);
  for (my $i = 0; $i < $_[0]; ++$i) { $result[ $i*$_[0] + $i ] = 1.0; }
  return @result;
}

sub matrix_element
{
# arguments: dim1 dim2 index1 index2 matrix
  return $_[4 + $_[2]*$_[0] + $_[3] ];
}

sub sum_matrices
{
# arguments: matrix1 matrix2
  return sum_vectors(@_);
}

sub transposed_matrix
{
# arguments: dim1 dim2 matrix
  my @result;
  for (my $i = 0; $i < $_[0]; ++$i)
  {
    for (my $j = 0; $j < $_[1]; ++$j)
    {
      $result[$_[0]*$j + $i] = $_[2 + $_[1]*$i + $j ];
    }
  }
  return @result;
}

sub mult_matrices
# arguments: dimension1 dimension2 dimension3 matrix1 matrix2
#  
#       d2                                       d3
#  +---------+              d3             +-----------+
#  |         |        +-----------+        |           |
#  |         |        |           |        |           |
#  |   M1    | d1 *   |    M2     | d2  =  |           | d1
#  |         |        |           |        |           |
#  |         |        +-----------+        |           |
#  +---------+                             +-----------+
{  
  my @result = zero_matrix($_[2], $_[0]);
  for (my $i = 0; $i < $_[0]; ++$i)
  {
    for (my $j = 0; $j < $_[2]; ++$j)
    {
      for (my $k = 0; $k < $_[1]; ++$k) { $result[$i*$_[2] + $j ] += $_[ 3 + $_[1]*$i + $k ] * $_[ 3 + $_[0]*$_[1] + $_[2]*$k + $j ]; }
    }
  }
  return @result;
}

sub mult_matrix_vector_left
# arguments: dim1 dim2 matrix vector
#       d2              1     
#  +----------+        +-+    
#  |          |        | |    
#  |          | d1 *   | | d2 
#  |          |        | |    
#  |          |        | |    
#  +----------+        +-+  
{
  return mult_matrices($_[0], $_[1], 1, @_[2..$#_]);
} 

sub mult_matrix_vector_right
# arguments: dim1 dim2 vector matrix
#                      d2
#                 +----------+           
#      d1         |          |         
# +---------+  *  |          | d1   
# +---------+     |          |          
#                 |          |          
#                 +----------+     
{
  return mult_matrices(1, $_[0], $_[1], @_[2..$#_] );
} 

sub minor
# arguments: dim1 dim2 index1 index2 matrix
{
  my @result;
  my $skip_i = 0, $skip_j = 0;
  for (my $i = 0; $i < $_[0] - 1; ++$i)
  {
    $skip_j = 0;
    if ($_[2] == $i) { $skip_i = 1; } ;
    for (my $j = 0; $j < $_[1] - 1; ++$j)
    {
      if ($_[3] == $j) {$skip_j = 1;}   ;
      $result[($_[1] - 1)*$i + $j] = $_[4 + $_[1]*($skip_i + $i) + $skip_j + $j ];
    } 
  }
  return @result;
}

sub swap_rows
# arguments: dim1 dim2 line1 line2 matrix
{
  my @result = @_[4..$#_];
  my $tmp;
  for (my $j = 0; $j < $_[1]; ++$j)
  {
    $tmp = $result[$_[1]*$_[2] + $j];
    $result[$_[1]*$_[2] + $j] = $result[$_[1]*$_[3] + $j];
    $result[$_[1]*$_[3] + $j] = $tmp;
  }
  return @result;
}

sub swap_cols
# arguments: dim1 dim2 column1 column2 matrix
{
  my @result = @_[4..$#_];
  my $tmp;
  for (my $i = 0; $i < $_[0]; ++$i)
  {
    $tmp = $result[$_[0]*$i + $_[2]];
    $result[$_[0]*$i + $_[2]] = $result[$_[0]*$i + $_[3]];
    $result[$_[0]*$i + $_[3]] = $tmp;
  }
  return @result;
}

sub det
# arguments: matrix
{
  my $dim = sqrt( @_ );
  my @matrix = @_;
  my $result = 1.0;
  # forward elimination
  for (my $j = 0; $j < $dim; ++$j )
  {
    # selecting pivot element
    my $max = $j;
    for (my $i = $j + 1; $i < $dim; ++$i)
    {
      if ( abs( $matrix[$dim*$i + $j] ) > abs( $matrix[$dim*$max + $j] ) ) { $max = $i; }
    }
    if ($max != $j) 
    { 
      @matrix = swap_rows($dim, $dim, $max, $j, @matrix); 
    }
    
    # performing step
    for (my $i = $j + 1; $i < $dim; ++$i)
    {
      if ($matrix[$dim*$j + $j] != 0) 
      {
        my $factor = $matrix[$dim*$i + $j] / $matrix[$dim*$j + $j];
        for (my $k = $j+1; $k < $dim; ++$k)
        {
          $matrix[$dim*$i + $k] -= $matrix[$dim*$j + $k] * $factor;
        }
        for (my $k = 0; $k < $dim; ++$k) 
        {
          $result[$dim*$i + $k] -= $result[$dim*$j + $k] * $factor;
        }
      } else {return 0;}
    }
    # calculating determinant
    for (my $i; $i < $dim; ++$i) { $result *= $matrix[$i*$dim + $i]; }
  }  
  return $result;
}

sub cofactor
# arguments: index1 index2 matrix
{
   my $dim = sqrt(@_ - 2);
   my $factor = ( $_[0] + $_[1] ) / 2 - int( ( $_[1] + $_[0] ) / 2 );
   return ( ($factor == 0) ? 1 : -1 ) * det( minor( $dim, $dim, $_[0], $_[1], @_[2..$#_] ) );
}

sub inversed_matrix
# arguments: matrix
{
  my $dim = sqrt(@_);
  my @matrix = @_;
  my @result = identity_matrix($dim);

  # forward elimination
  for (my $j = 0; $j < $dim; ++$j )
  {
    # selecting pivot element
    my $max = $j;
    for (my $i = $j + 1; $i < $dim; ++$i)
    {
      if ( abs( $matrix[$dim*$i + $j] ) > abs( $matrix[$dim*$max + $j] ) ) { $max = $i; }
    }
    if ($max != $j) 
    { 
      @matrix = swap_rows($dim, $dim, $max, $j, @matrix); 
      @result = swap_rows($dim, $dim, $max, $j, @result); 
    }
    
    # performing step
    for (my $i = $j + 1; $i < $dim; ++$i)
    {
      if ($matrix[$dim*$j + $j] != 0) 
      {
        my $factor = $matrix[$dim*$i + $j] / $matrix[$dim*$j + $j];
        for (my $k = 0; $k < $dim; ++$k)
        {
          $matrix[$dim*$i + $k] -= $matrix[$dim*$j + $k] * $factor;
          $result[$dim*$i + $k] -= $result[$dim*$j + $k] * $factor;
        }
      }
    }
  }
  # backward elimination
  # making top triangle beeing equal 0
  
  for (my $i = $dim - 1; $i >= 0 ; --$i)
  {
    for (my $k = 0; $k < $i; ++$k)
    {
      if ($matrix[$dim*$i + $i] != 0)
      {
        my $factor = $matrix[$dim*$k + $i] / $matrix[$dim*$i + $i];
        for (my $j = 0; $j < $dim; ++$j)
        {
          $result[$dim*$k + $j] -= $result[$dim*$i + $j] * $factor; 
        }
      }
    }
  }
  # making diagonal beeing equal 1
  for (my $i = 0; $i < $dim; ++$i)
  {
    for (my $j = 0; $j < $dim; ++$j)
    {
      $result[$dim*$i + $j] /= $matrix[$dim * $i + $i];
    } 
  }
  return @result;  
}

# Convenience functions for 3D space transformation.
# Note that matrices are 4x4 and vectors are 4 dimensional

sub translation_matrix
{
# arguments: translation vector 
  my @result = identity_matrix(4);
  $result[3] = $_[0];
  $result[7] = $_[1];
  $result[11]= $_[2];
  return @result;
} 

sub rotation_matrix
{
# arguments: angle, axis
  my @result;
  my $c = cos($_[0]), $s = sin($_[0]);
  my @axis = normalized_vector(@_[1,2,3]);
  my @axis_squared = ( $axis[0] * $axis[0], $axis[1] * $axis[1], $axis[2] * $axis[2] );
  $result[0] = $axis_squared[0] + (1.0 - $axis_squared[0]) * $c;
  $result[1] = $axis[0]*$axis[1] * (1.0 - $c) - $axis[2]*$s;
  $result[2] = $axis[0]*$axis[2] * (1.0 - $c) + $axis[1]*$s;
  $result[3] = 0.0;
  $result[4] = $axis[0]*$axis[1] * (1.0 - $c) + $axis[2]*$s;
  $result[5] = $axis_squared[1] + (1.0 - $axis_squared[1])*$c;
  $result[6] = $axis[1]*$axis[2]*(1.0 - $c) - $axis[0]*$s;
  $result[7] = 0.0;
  $result[8] = $axis[0]*$axis[2]*(1.0 - $c) - $axis[1] * $s;
  $result[9] = $axis[1]*$axis[2]*(1.0 - $c) + $axis[0] * $s;
  $result[10] = $axis_squared[2] + (1.0 - $axis_squared[2])*$c;
  $result[11] = 0.0;
  $result[12] = 0.0;
  $result[13] = 0.0;
  $result[14] = 0.0;
  $result[15] = 1.0;
  return @result;  
}

sub scale_matrix
{
# arguments: scale_vector (3D)
  @result = zero_matrix(4, 4);
  $result[0] = $_[0];
  $result[5] = $_[1];
  $result[10]= $_[2];
  $result[15]= $_[3];
  return @result;
}

sub inversion_matrix
{
# no arguments
  return (-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1);
}

# Print utilities (return strings)


sub print_vector
# arguments: vector
{
  my $result = $_[0];
  foreach(@_[1..$#_]) { $result .= "\t".$_; }
  return $result;
}

sub print_matrix
# arguents: dim1 dim2 matrix
{
  my $result = "", $dim1 = $_[0], $dim2 = $_[1];
  my @mat = @_[2..$#_];
  for (my $i = 0; $i < $dim1 ; ++$i)
  {
    $result .= print_vector(@_[($dim2*$i+2)..($dim2*($i+1)+1)])."\n";
  }
  return $result;
}

# Nonmatematical functions

sub insert_rows
# arguments: dim1 dim2 rowIndex matrix rows
#
#           d2
#      +-----------+
#      |           |
#  d1  |  matrix   | 
#      +-----------+                 d2
#                   <-rowIndex +------------+
#                 <------------|    rows    | 
#                              +------------+
#      +-----------+
#      |           |
#      +-----------+
{
  return ( @_[3..(2+$_[2]*$_[1])], @_[(3+$_[0]*$_[1])..$#_], @_[(3+$_[2]*$_[1])..(2+$_[0]*$_[1])] );
}

sub insert_cols
# arguments: dim1 dim2 colIndex matrix cols
{
  my $dim1 = $_[0], $dim2 = $_[1], $col = $_[2];
  my @mat = @_[3..(2+$dim1*$dim2)];
  my @cols = @_[(3+$dim1*$dim2)..$#_];
  my $width = @cols / $dim1;
  my @result;
  for (my $i = 0; $i < $dim1; ++$i)
  {
    push @result, (@mat[$i*$dim2..($i*$dim2 + $col - 1)], @cols[$i*$width..(($i+1)*$width-1)], @mat[($i*$dim2 + $col)..(($i+1)*$dim2 - 1)] );
  }
  return @result;
}

sub remove_rows
# arguments: dim1 dim2 rowIndex width matrix
{
  return ( @_[4..(3+$_[2]*$_[1])], @_[(4+($_[2] + $_[3])*$_[1] )..$#_] );
}

sub remove_cols
# arguments: dim1 dim2 colIndex width matrix
{
  my $dim1 = $_[0], $dim2 = $_[1], $col = $_[2], $width = $_[3];
  my @mat = @_[4..$#_];
  my @result;
  for (my $i = 0; $i < $dim1; ++$i)
  {
    push @result, (@mat[$i*$dim2..($i*$dim2 + $col - 1)], @mat[($i*$dim2 + $col + $width)..(($i+1)*$dim2 - 1)] );
  }
  return @result;
}

sub extract_rows
# arguments: dim1 dim2 rowIndex width matrix
{
  return @_[(4+$_[1]*$_[2])..(3+$_[1]*($_[2]+$_[3]))];
}

sub extract_cols
# arguments: dim1 dim2 colIndex width matrix
{
  my $dim1 = $_[0], $dim2 = $_[1], $col = $_[2], $width = $_[3];
  my @mat = @_[4..$#_];
  my @result;
  for (my $i = 0; $i < $dim1; ++$i)
  {
    push @result, @mat[($i*$dim2 + $col)..($i*$dim2 + $col + $width - 1)] ;
  }
  return @result;
}

sub solve_lin_sys
# arguments: dim A B
#
# solves linear system of equations AX=B 
# where A is a square matrix of the size dim x dim
{
  return mult_matrix_vector_left($_[0], $_[0], inversed_matrix(@_[1..$_[0]*$_[0]]), @_[$_[0]*$_[0]+1..$#_]);
}

sub make_least_squares
# arguments dim1 dim2 A B
#
#       d2              1           1
#  +----------+        +-+         +-+ 
#  |          |        | |         | | 
#  |          | d1 *   | | d2  =   | | d1
#  |          |        | |         |B| 
#  |    A     |        | |         | | 
#  |          |        +-+         | |
#  |          |                    | |
#  +----------+                    +-+
{
  my $d1 = $_[0];
  my $d2 = $_[1];
  my @A = @_[2..1+$d1*$d2];
  my @B = @_[2+$d1*$d2..$#_];

  my @mat = zero_matrix($d2, $d2);
  for (my $i = 0; $i < $d2; ++$i)
  {
    for (my $j = $i; $j < $d2; ++$j)
    {
      for (my $k = 0; $k < $d1; ++$k)
      {
        $mat[$i*$d2+$j] += $A[$k*$d2+$i]*$A[$k*$d2+$j];
      }
      $mat[$j*$d2+$i] = $mat[$i*$d2+$j];
    }
  }

  my @vec = zero_matrix(1, $d2);
  for (my $i = 0; $i < $d2; ++$i)
  {
    for (my $k = 0; $k < $d1; ++$k)
    {
      $vec[$i] += $B[$k]*$A[$k*$d2+$i];
    }
  }
 
  my @result = solve_lin_sys($d2, @mat, @vec);

  my $deviation = 0;
  for (my $i = 0; $i < $d1; ++$i)
  {
    my $res = 0;
    for (my $j = 0; $j < $d2; ++$j)
    {
      $res += $A[$i*$d2+$j]*$result[$j];
    }
    $deviation += ($res - $B[$i])*($res - $B[$i]);
  }
 
  return (sqrt($deviation / $d1), @result);
}


return 1;

END {}


                                                   